#pragma once

#include "VisioCommon.h"
#include <opencv2\opencv.hpp>
#include "..\MathAux\Matrix\matrix.h"
#include "Camera.h"

#define MIN_VIEWS_FOR_TRIANGULATION 2
#define MAX_REPROJECTION_ERROR 16.0000

using namespace cv;

class CTriangulator
{
public:

	Point3d Triangulate(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB);
	Point3d TrinagulateN( Point2d* pP, CCamera** ppCams, int NumPoints, double& error );
	Point3d TriangulateLinear(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB);
	Point3d RefineTriangulateNonLinear(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB, Point3d _x);
	Point3d TriangulateInternal(Point2d p, Point2d q, CCamera& CamA, CCamera& CamB);
	Point3d TriangulateNInternal(double* p, CCamera** ppCams, int NumPoints);
	Point3d TriangulateNLinear(double* p, CCamera** ppCams, int NumPoints);
	Point3d RefineTriangulateNNonLinear(double* p, CCamera** ppCams, int NumPoints, Point3d _x);
	Point3d TriangulateBundler(double* p, double* q, Mat R1, Mat t1, Mat R2, Mat t2, double* error);
private:
};